/* Vecmath.

   Copyright (C) 2001, 2002, 2003 Stefan Maierhofer.

   Written by Stefan Maierhofer <sm@cg.tuwien.ac.at>

   This file is part of Vecmath.

   Vecmath is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   Vecmath is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with Vecmath; if not, write to the Free Software Foundation,
   Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */

using System;
using NUnit.Framework;

namespace Vecmath
{

    /// <summary>
    /// Rot3D.
    /// </summary>
    public struct Rot3D
    {

        public static Rot3D Identity = new Rot3D(0, Vec3D.UnitZ);

        public Rot3D(double radians, Vec3D axis)
        {
            axis.Normalize();
            r = Math.Cos(radians/2.0);
            v = axis.Normalized * Math.Sin(radians/2.0);
        }

        public Rot3D(Vec3D from, Vec3D to)
        {
            from.Normalize();
            to.Normalize();

            double cost =
                Vec3D.Dot(from, to) /
                Math.Sqrt(Vec3D.Dot(from, to) * Vec3D.Dot(to, to));

            if (cost > 0.99999)
            {
                r = 1;
                v = new Vec3D(0, 0, 0);
            }
            else if (cost < -0.99999)
            {
                Vec3D frm = from.Normalized;
                v = Vec3D.Cross(frm, Vec3D.UnitX);
                if (v.Length < 0.00001) v = Vec3D.Cross(frm, Vec3D.UnitY);
                r = 0;
                v.Normalize();
            }
            else
            {
                r = Math.Sqrt(0.5 * (1.0 + cost));
                v = Vec3D.Cross(from, to);
                v *= Math.Sqrt((0.5 * (1.0 - cost))/Vec3D.Dot(v, v));
            }
        }

        public Trafo3D ToTrafo3D()
        {
            return new Trafo3D(
                1.0-2.0*(v[1]*v[1] + v[2] * v[2]),
                2.0*(v[0]*v[1] + v[2] * r),
                2.0*(v[0]*v[2] - v[1] * r),
                0.0,

                2.0*(v[0]*v[1] - v[2] * r),
                1.0-2.0*(v[0]*v[0] + v[2] * v[2]),
                2.0*(v[1]*v[2] + v[0] * r),
                0.0,

                2.0*(v[0]*v[2] + v[1] * r),
                2.0*(v[1]*v[2] - v[0] * r),
                1.0-2.0*(v[0]*v[0] + v[1] * v[1]),
                0.0,

                0.0, 0.0, 0.0, 1.0

                );
        }

        public static implicit operator Trafo3D(Rot3D r)
        {
            return r.ToTrafo3D();
        }

        public static Vec3D operator*(Vec3D v, Rot3D r)
        {
            return v * r.ToTrafo3D();
        }

        public static Pnt3D operator*(Pnt3D p, Rot3D r)
        {
            return p * r.ToTrafo3D();
        }

        public static Ray3D operator*(Ray3D r, Rot3D t)
        {
            return new Ray3D(r.p * t, r.v * t);
        }

        public static Box3D operator*(Box3D b, Rot3D r)
        {
            Box3D result = Box3D.Empty;
            result += b.LLL * r;
            result += b.LLU * r;
            result += b.LUL * r;
            result += b.LUU * r;
            result += b.ULL * r;
            result += b.ULU * r;
            result += b.UUL * r;
            result += b.UUU * r;
            return result;
        }

        public static Trafo3D operator*(Shift3D t, Rot3D r)
        {
            return t.ToTrafo3D() * r.ToTrafo3D();
        }

        public static Trafo3D operator*(Scale3D t, Rot3D r)
        {
            return t.ToTrafo3D() * r.ToTrafo3D();
        }

        private Vec3D v;
        private double r;

    }




    [TestFixture]
    public class Rot3DTest
    {
       
        [Test]
        public void MulPnt()
        {
            Rot3D t = new Rot3D(Math.PI, new Vec3D(0,0,1));
            Pnt3D p = new Pnt3D(1,2,3);
            Assert.IsTrue(Vecmath.ApproxEquals(new Pnt3D(-1,-2,3), p * t));

            t = new Rot3D(Math.PI, new Vec3D(0,1,0));
            Assert.IsTrue(Vecmath.ApproxEquals(new Pnt3D(-1,2,-3), p * t));

            t = new Rot3D(Math.PI, new Vec3D(1,0,0));
            Assert.IsTrue(Vecmath.ApproxEquals(new Pnt3D(1,-2,-3), p * t));
        }

        [Test]
        public void MulVec()
        {
            Rot3D t = new Rot3D(Math.PI, new Vec3D(0,0,1));
            Vec3D v = new Vec3D(1,2,3);
            Assert.IsTrue(Vecmath.ApproxEquals(new Vec3D(-1,-2,3), v * t));

            t = new Rot3D(Math.PI, new Vec3D(0,1,0));
            Assert.IsTrue(Vecmath.ApproxEquals(new Vec3D(-1,2,-3), v * t));

            t = new Rot3D(Math.PI, new Vec3D(1,0,0));
            Assert.IsTrue(Vecmath.ApproxEquals(new Vec3D(1,-2,-3), v * t));
        }

    }

}
